#include <stddef.h>
#include <string.h>
#include <malloc.h>
#include <log2.h>
#include <types.h>
#include <sys/fifo.h>
#include <sys/spinlock.h>

void __fifo_reset(fifo_t *fifo)
{
    fifo->in = fifo->out = 0;
}

uint64_t __fifo_len(fifo_t *fifo)
{
    return fifo->in - fifo->out;
}

uint64_t __fifo_put(fifo_t *fifo, uint8_t *buff, uint64_t len)
{
    uint64_t size;

    len = min(len, fifo->size - fifo->in + fifo->out);
    size = min(len, fifo->size - (fifo->in & (fifo->size - 1)));
	memcpy(fifo->buffer + (fifo->in & (fifo->size - 1)), buff, size);
	memcpy(fifo->buffer, buff + size, len - size);
	fifo->in += len;
	return len;
}

uint64_t __fifo_get(fifo_t *fifo, uint8_t *buff, uint64_t len)
{
    uint64_t size;

    len = min(len, fifo->in - fifo->out);
    size = min(len, fifo->size - (fifo->out & (fifo->size - 1)));
    memcpy(buff, fifo->buffer + (fifo->out & (fifo->size - 1)), size);
    memcpy(buff + size, fifo->buffer, len - size);
    fifo->out += len;

    return len;
}

fifo_t *fifo_alloc(uint64_t size)
{
    fifo_t *fifo;

    if (size & (size - 1))
        size = roundup_pow_of_two(size);

    fifo = malloc(sizeof(fifo_t));
    if (!fifo)
        return NULL;

    fifo->buffer = malloc(size);
    if (!fifo->buffer)
    {
        free(fifo);
        return NULL;
    }
    fifo->size = size;
    fifo->in = 0;
    fifo->out = 0;
    spin_lock_init(&fifo->lock);
    return fifo;
}

void fifo_free(fifo_t *fifo)
{
    if (fifo)
    {
        free(fifo->buffer);
        free(fifo);
    }
}

void fifo_reset(fifo_t *fifo)
{
    if (!fifo)
        return;
    spin_lock(&fifo->lock);
    __fifo_reset(fifo);
    spin_unlock(&fifo->lock);
}

uint64_t fifo_len(fifo_t *fifo)
{
    uint64_t len;

    if (!fifo)
        return 0;
    spin_lock(&fifo->lock);
    len = __fifo_len(fifo);
    spin_unlock(&fifo->lock);

    return len;
}

uint64_t fifo_get(fifo_t *fifo, uint8_t *buff, uint64_t len)
{
    uint64_t ret;

    if (!fifo || !buff)
        return 0;
    spin_lock(&fifo->lock);
    ret = __fifo_get(fifo, buff, len);
    if (fifo->in == fifo->out)
        fifo->in = fifo->out = 0;
    spin_unlock(&fifo->lock);
    return ret;
}

uint64_t fifo_put(fifo_t *fifo, uint8_t *buff, uint64_t len)
{
    int ret;
    if (!fifo || !buff)
        return 0;
    spin_lock(&fifo->lock);
    ret = __fifo_put(fifo, buff, len);
    spin_unlock(&fifo->lock);
    return ret;
}
